
#ifndef UTIL_H
#define UTIL_H

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/random_sample.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/surface/mls.h>

#include <ros/ros.h>
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

using PT_TYPE = pcl::PointXYZ; 
using PointT = pcl::PointXYZ;
using PointXYZI = pcl::PointXYZI;
using PointCloudT = pcl::PointCloud<PointT>;


//定义一个通用的点云格式
typedef struct PCMessage
{  
  long   uniTime;
  PointCloudT pc;
}stPCMessage;

//定义一个通用的IMU格式
typedef struct ImuMessage
{
  long   uniTime;
  double time; //second
  float  linear_acceleration[3];
  float  angular_velocity[3];
  float  rpy[3];        //eular angle
  float  quaternion[4]; //w,x,y,z
}stImuMessage;


// //velodyne
// struct PointXYZIRT
// {
//     PCL_ADD_POINT4D
//     PCL_ADD_INTENSITY;
//     uint16_t ring;
//     float time;
//     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// } EIGEN_ALIGN16;

// POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT,  
//     (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
//     (uint16_t, ring, ring) (float, time, time)
// )

//point from PCAP
struct PointXYZIRT_PCAP
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT_PCAP,  
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint16_t, ring, ring) (double, time, time)
)

// // Ouster
// struct PointXYZIRT {
//     PCL_ADD_POINT4D;
//     float intensity;
//     uint32_t t;
//     uint16_t reflectivity;
//     uint8_t ring;
//     //uint16_t noise;
//     uint32_t range;
//     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// }EIGEN_ALIGN16;
// // POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
// //     (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
// //     (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
// //     (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
// // )
// POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
//     (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
//     (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
//     (uint8_t, ring, ring) (uint32_t, range, range)
// )

//模板函数必须全部放在头文件中
template <typename PointType>
void VoxelFilter(pcl::PointCloud<PointType> &pc, double vxsize = 0.1)
{
    //voxel filter
    pcl::VoxelGrid<PointType> vf;
    vf.setInputCloud(pc.makeShared());
    vf.setLeafSize(vxsize, vxsize, vxsize);
    //vf.setMinimumPointsNumberPerVoxel(2);
    pcl::PointCloud<PointType> voxel_filtered;
    vf.filter(voxel_filtered);
    pc = voxel_filtered;
}

template <typename PointType>
void RandomFilter(pcl::PointCloud<PointType> &pc, double ratio)
{
    int inputSize = pc.size();
    int outSize = inputSize * ratio;

    pcl::RandomSample<PointType> rs;
    rs.setInputCloud(pc.makeShared());
    rs.setSample(outSize);
    pcl::PointCloud<PointType> cloud_out;
    rs.filter(cloud_out);
    pc = cloud_out;
}

template <typename PointType>
double gicpMatchWithPredict( pcl::PointCloud<PointType> &src, 
                             pcl::PointCloud<PointType> &dst, 
                             Eigen::Matrix4f initPose, 
                             Eigen::Matrix4f& finalPose)
{
    double score = 0;
    //auto t1 = std::chrono::high_resolution_clock::now();    
    //printf("%d %d \n", src.size(), dst.size());
    pcl::PointCloud<PointType> aligned;
    pcl::GeneralizedIterativeClosestPoint<PointType, PointType> pcl_gicp;
    //pcl_gicp.setMaxCorrespondenceDistance(0.5);
    pcl_gicp.setInputTarget(dst.makeShared());
    pcl_gicp.setInputSource(src.makeShared());
    pcl_gicp.align(aligned, initPose);
    finalPose = pcl_gicp.getFinalTransformation();
    score = pcl_gicp.getFitnessScore(5.0);   
    //src = *aligned;
    //auto t2 = std::chrono::high_resolution_clock::now();
    //double single = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    //std::cout << "pcl gicp :" << single << "[msec] " << std::endl;
    return score;
}


//ndt默认的浮点类型是float，所以初始化的矩阵需要设置为float类型
template <typename PointT>
double NDTMatch(pcl::PointCloud<PointT>& source,
                pcl::PointCloud<PointT>& target,
                Eigen::Matrix4f  init_guess,
                Eigen::Matrix4f& mt
                )
{
    // Initializing Normal Distributions Transform (NDT).
    pcl::NormalDistributionsTransform<PointT, PointT> ndt;
    // Setting scale dependent NDT parameters   
    // Setting minimum transformation difference for termination condition.
    ndt.setTransformationEpsilon(0.01);
    // Setting maximum step size for More-Thuente line search.
    ndt.setStepSize(0.1);
    //Setting Resolution of NDT grid structure (VoxelGridCovariance).
    ndt.setResolution(1.0);
    // Setting max number of registration iterations.
    ndt.setMaximumIterations(16);
    // Setting point cloud to be aligned.
    ndt.setInputSource(source.makeShared());
    // Setting point cloud to be aligned to.
    ndt.setInputTarget(target.makeShared());
    // Calculating required rigid transform to align the input cloud to the target cloud.
    pcl::PointCloud<PointT> output_cloud; // (new pcl::PointCloud<PT_TYPE>);
    ndt.align(output_cloud, init_guess);
    //mt = ndt.getFinalTransformation();
    mt = ndt.getFinalTransformation();
   
    return ndt.getFitnessScore(5.0);
}     

template <typename PointType>
void SavePointCloudToPly(std::string filename, pcl::PointCloud<PointType>& pointcloud)
{
    pcl::PLYWriter writer;
    writer.write(filename, pointcloud);
}

template <typename T>
void imuRPY2rosRPY(const sensor_msgs::Imu::ConstPtr thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
    double imuRoll, imuPitch, imuYaw;

    tf::Quaternion orientation;
    tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);

    *rosRoll = imuRoll;
    *rosPitch = imuPitch;
    *rosYaw = imuYaw;
}


Eigen::Matrix3f     generateRotationMatrix(float IMU_roll, float IMU_pitch, float IMU_yaw);
Eigen::Quaterniond  Euler_to_Quaterniond(double yaw, double pitching, double droll);
Eigen::Vector3d     Quaterniond_to_Euler(double x, double y, double z, double w);
void gacc_to_att(float* gravity_acc, float* ret_roll, float *ret_pitch);
Eigen::Matrix3f     EulerToRotationMatrix(float roll, float pitch, float yaw);


#endif